#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import socket

linear_motor_list = [13,14,23,24,15,16,25,26,34,44,36,37,46,47]
rotary_motor_list = [72,12,22,11,21,31,32,33,35,41,42,43,45,71]

ip_mapping = {
    31: '169.254.61.243',
    32: '169.254.61.243',
    33: '169.254.61.240',
    34: '169.254.61.242',
    35: '169.254.61.240',
    36: '169.254.61.240',
    37: '169.254.61.240',
    41: '169.254.61.243',
    42: '169.254.61.243',
    43: '169.254.61.246',
    44: '169.254.61.242',
    45: '169.254.61.246',
    46: '169.254.61.246',
    47: '169.254.61.246',
    11: '169.254.61.244',
    12: '169.254.61.242',
    13: '169.254.61.245',
    14: '169.254.61.245',
    15: '169.254.61.245',
    16: '169.254.61.245',
    21: '169.254.61.244',
    22: '169.254.61.242',
    23: '169.254.61.241',
    24: '169.254.61.241',
    25: '169.254.61.241',
    26: '169.254.61.241',
    71: '169.254.61.244',
    72: '169.254.61.244',
}


# 电机对应端口
port_mapping = {
    31: 1032,
    32: 1031,
    33: 1039,
    34: 1037,
    35: 1038,
    36: 1030,
    37: 1031,
    41: 1038,
    42: 1037,
    43: 1039,
    44: 1036,
    45: 1038,
    46: 1030,
    47: 1031,
    11: 1031,
    12: 1033,
    13: 1039,
    14: 1040,
    15: 1031,
    16: 1032,
    21: 1030,
    22: 1034,
    23: 1039,
    24: 1040,
    25: 1031,
    26: 1032,
    71: 1038,
    72: 1039,
}


# 设置减速比
ratio_mapping = {
    31: 81,
    32: 81,
    33: 81,
    34: 80,
    35: 81,
    36: 120,
    37: 120,
    41: 81,
    42: 81,
    43: 81,
    44: 80,
    45: 81,
    46: 120,
    47: 120,
    11: 81,
    12: 101,
    13: 40,
    14: 40,
    15: 80,
    16: 80,
    21: 81,
    22: 101,
    23: 40,
    24: 40,
    25: 80,
    26: 80,
    71: 81,
    72: 101,
}

# 公共模块部分
def hex2bin(hex_str: str, bin_width: int = -1):
    hex_str = hex_str.strip()
    if hex_str[0] in ['+', '-', '_'] or hex_str[-1] == '_' or '__' in hex_str:
        print('输入不合法，必须为16进制补码，不允许带正负号，首尾不能是下划线，不能连续出现两个下划线')
    elif hex_str[:2].lower() == '0x':
        hex_str = hex_str[2:]
    hex_str = hex_str.replace('_', '')
    bin_str = ''.join([oneHex2fourBin(s) for s in hex_str])

    for i in range(len(bin_str) - 1):
        if bin_str[i + 1] == bin_str[0]:
            if i + 1 == len(bin_str) - 1:
                bin_str = bin_str[i:]
            else:
                continue
        else:
            bin_str = bin_str[i:]
            break
    if bin_str == '00':
        bin_str = '0'
    if bin_width == -1:
        pass
    elif bin_width < len(bin_str):
        print('位宽参数<16进制补码，请修正位宽')
    else:
        bin_str = bin_str[0] * (bin_width - len(bin_str)) + bin_str
    return '0b' + bin_str


def dec2hex(data: int):
    return hex(data % 256)


def bin2dec(bin_str: str):
    bin_str = bin_str.strip()
    if bin_str[:2] == '0b':
        if bin_str[2] == '_':
            bin_str = bin_str[3:]
        else:
            bin_str = bin_str[2:]
    if bin_str[0] == '_':
        print('输入不合法，首字符不能是下划线，且不允许出现连续两个下划线')
    elif bin_str[0] == '0':
        return int(bin_str, base=2)
    elif bin_str[0] == '1':
        a = int(bin_str, base=2)
        bin_str = bin_str.replace('_', '')
        return a - 2 ** len(bin_str)
    else:
        print('输入不合法，必须为2进补码，不允许带正负号')


def oneHex2fourBin(one_hex: str):
    raw = {'A': '1010', 'B': '1011', 'C': '1100', 'D': '1101', 'E': '1110', 'F': '1111'}
    if one_hex in [str(i) for i in range(10)]:
        return '{:0>4}'.format(str(bin(int(one_hex)))[2:])
    elif one_hex.upper() in ['A', 'B', 'C', 'D', 'E', 'F']:
        return raw[one_hex.upper()]
    else:
        print('十六进制格式错误, 只能包含0~9， a~f，A~F')


# 正负值转换为16进制
def transformVal(val):
    if val > 0:
        low_hex = hex(val).replace('0x', '')
        if len(low_hex) == 3:
            low_angle = low_hex[1:3]
            high_angle = '0%s' % low_hex[0]
        elif len(low_hex) == 2:
            low_angle = low_hex
            high_angle = '0'
        elif len(low_hex) == 1:
            low_angle = '0'
            high_angle = '0'
        else:
            low_angle = low_hex[2:4]
            high_angle = low_hex[0:2]
        return high_angle, low_angle
    elif val < 0:
        low_iq = (hex(16 ** 7 + val)[-2:])
        high_iq = (hex(16 ** 7 + val)[-4:-2])
        return high_iq, low_iq
    else:
        return 0, 0


# 新版电机驱动，包含：腿部、手臂和腰部，驱动器保持一致
class RotaryMotorNet:

    def __int__(self):
        pass

    def __del__(self):
        pass


    # 8、	位置闭环控制命令（常用）
    def dian_control_command(self, did, angleControl, speed, acceleration, ratio):
        info_list = []
        # 命令字节
        info_list.append('0x80')
        # 计算出关节运动实际运动度数：目标度数 * 减速比
        real_angleControl = int(angleControl * ratio)
        # print(did, real_angleControl, speed, acceleration)

        if real_angleControl >= 0:
            angleControl_hex = hex(real_angleControl).replace('0x', '')
            # print(angleControl_hex)
            if len(angleControl_hex) == 3:
                low_angle = angleControl_hex[1:3]
                high_angle = '0%s' % angleControl_hex[0]
            elif len(angleControl_hex) == 2:
                low_angle = angleControl_hex
                high_angle = '0'
            elif len(angleControl_hex) == 1:
                low_angle = '0'
                high_angle = '0'
            else:
                low_angle = angleControl_hex[2:4]
                high_angle = angleControl_hex[0:2]
            #位置控制低字节1
            info_list.append(hex(0))
            #位置控制字节2
            info_list.append(hex(0))
            #位置控制字节3
            info_list.append(high_angle)
            #位置控制字节4
            info_list.append(low_angle)
        else:
            low_iq = (hex(16 ** 7 + real_angleControl)[-2:])
            high_iq = (hex(16 ** 7 + real_angleControl)[-4:-2])

            #位置控制低字节1
            info_list.append('ff')
            #位置控制字节2
            info_list.append('ff')
            #位置控制字节3
            info_list.append(high_iq)
            #位置控制字节4
            info_list.append(low_iq)

        # 5： 最大转速高字节(50-900) 、 6：最大转速低字节
        try:
            speed_hex = hex(speed).replace('0x', '')
            if len(speed_hex) == 3:
                low_angle = speed_hex[1:3]
                high_angle = '0%s' % speed_hex[0]
            elif len(speed_hex) == 2:
                low_angle = speed_hex
                high_angle = '0'
            elif len(speed_hex) == 1:
                low_angle = '0'
                high_angle = '0'
            else:
                low_angle = speed_hex[2:4]
                high_angle = speed_hex[0:2]
            info_list.append(high_angle)
            info_list.append(low_angle)
        except:
            info_list.append(hex(0))
            info_list.append(hex(0))

        # 7： 加速度（10-200）
        info_list.append(hex(acceleration).replace('0x', ''))

        convert_list = []
        for str in info_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    # 电机发送
    def dian_send_command(self, ip, port, command):
        try:
            msg = bytearray.fromhex(command)
            udp_monitor_addr1 = (ip, port)
            udp_monitor_sock1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
            udp_monitor_sock1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
            udp_monitor_sock1.sendto(msg, udp_monitor_addr1)
        except:
            print('电机运行命令发送失败...')


    # 电机电流返回值
    def dian_send_command_r1(self, did, ip, port, command):
        rlt = (0, 0)
        msg = bytearray.fromhex(command)
        udp_monitor_addr1 = (ip, port)
        udp_monitor_sock1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        udp_monitor_sock1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        udp_monitor_sock1.sendto(msg, udp_monitor_addr1)
        udp_monitor_sock1.settimeout(0.01)  # 请求超时时间，不设置会等待30s超时
        msg, address = udp_monitor_sock1.recvfrom(1024)

        if msg is not None and len(msg) > 0:
            result_recv_data = msg.hex()
            ls = []
            for i in range(len(result_recv_data) // 2):
                ls.append(result_recv_data[i * 2] + result_recv_data[i * 2 + 1])

            if len(ls) > 0:
                # 当前电电流度值
                t1_high = ls[0]
                t1_low = ls[1]
                cur = bin2dec(hex2bin((t1_high + t1_low)))
                rlt = (1, cur)

        return rlt


    # # 电机发送记录返回值
    def dian_send_command_r(self, did, ip, port, command):
        rlt = (0, 0.0)
        msg = bytearray.fromhex(command)

        udp_monitor_addr1 = (ip, port)
        udp_monitor_sock1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        udp_monitor_sock1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        udp_monitor_sock1.sendto(msg, udp_monitor_addr1)
        udp_monitor_sock1.settimeout(0.01) #请求超时时间，不设置会等待30s超时
        msg, address = udp_monitor_sock1.recvfrom(1024)

        if msg is not None and len(msg) > 0:
            result_recv_data = msg.hex()
            ls = []
            for i in range(len(result_recv_data) // 2):
                ls.append(result_recv_data[i * 2] + result_recv_data[i * 2 + 1])

            # print(ls)
            if len(ls) > 0:
                # 当前电机角度值
                t1_high = ls[2]
                t1_low = ls[3]
                # print(t1_high, t1_low)
                angle = bin2dec(hex2bin((t1_high + t1_low)))
                # 实际角度值，根据did匹配不同关节减速比
                ratio = ratio_mapping.get(did)
                real_angle = round(angle / ratio, 2)

                rlt = (1, real_angle)

        return rlt


    # # 电机发送记录返回值
    def dian_send_command_r2(self, did, ip, port, command):
        rlt = (0, (0, 0))
        msg = bytearray.fromhex(command)
        udp_monitor_addr1 = (ip, port)
        udp_monitor_sock1 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        udp_monitor_sock1.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        udp_monitor_sock1.sendto(msg, udp_monitor_addr1)
        udp_monitor_sock1.settimeout(0.01) #请求超时时间，不设置会等待30s超时
        msg, address = udp_monitor_sock1.recvfrom(1024)

        if msg is not None and len(msg) > 0:
            result_recv_data = msg.hex()
            ls = []
            for i in range(len(result_recv_data) // 2):
                ls.append(result_recv_data[i * 2] + result_recv_data[i * 2 + 1])

            if len(ls) > 0:
                # 当前电机角度值
                t1_high = ls[2]
                t1_low = ls[3]
                angle = bin2dec(hex2bin((t1_high + t1_low)))

                # 实际角度值，根据did匹配不同关节减速比
                ratio = ratio_mapping.get(did)
                real_angle = round(angle / ratio, 2)

                # 电机最小限位
                t2_high = ls[4]
                t2_low =  ls[5]
                limit_min = bin2dec(hex2bin((t2_high + t2_low)))

                # 电机最大限位
                t3_high = ls[6]
                t3_low =  ls[7]
                limit_max = bin2dec(hex2bin((t3_high + t3_low)))

                rlt = (1, (limit_min, limit_max))

        return rlt


    # 1、	电机停机，回待机状态命令
    def dian_stop_command(self):
        info_list = []
        # 命令字节
        info_list.append('0x10')

        # 空值
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))

        convert_list = []
        for str in info_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    #3、	设置零点命令（常用）
    def dian_zero_command(self):
        info_list = []
        # 命令字节
        info_list.append('0x21')

        # 空值
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))

        convert_list = []
        for str in info_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    # 7、	获取电机控制板信息（常用）
    def dian_data_command(self):
        info_list = []
        # 命令字节
        info_list.append('0x30')

        # 空值
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))

        convert_list = []
        for str in info_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    # 5、	设置限位命令（常用）
    def dian_limit_command(self, low, high):
        info_list = []
        # 命令字节
        info_list.append('0x23')

        # 低位
        v1, v2 = transformVal(low)
        info_list.append(v1)
        info_list.append(v2)
        # 高位
        v3, v4 = transformVal(high)
        info_list.append(v3)
        info_list.append(v4)
        # 其他空值
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))

        convert_list = []
        for str in info_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    # 电机电流
    def dian_cur_command(self):
        info_list = []
        # 命令字节
        info_list.append('0x31')
        # 空值
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))
        info_list.append(hex(0))

        convert_list = []
        for str in info_list:
            t_str = str.replace('0x', '')
            if len(t_str) == 1:
                convert_list.append('0%s' % t_str)
            else:
                convert_list.append(t_str)
        return ' '.join(convert_list)


    # 8、	位置闭环控制命令
    # 入参1：did = 电机ID
    # 入参2：angleControl = 运行角度
    # 入参3：maxSpeed = 运行速度，默认值5000，调整范围：1-15000
    # 入参4：acceleration = 加速度，默认值100，调整范围：1-200
    # 入参5：isPrint = 是否打印封装命令，True为打印、False为不打印，默认为False不打印
    def SetMotorData(self, did, angleControl, speed=5000, acceleration=100, isPrint=False):
        if did not in rotary_motor_list:
            print('-- 旋转电机ID错误，输入电机ID：%s' % did)
            return

        # 根据did匹配不同ip
        ip = ip_mapping.get(did)
        # 根据did匹配不同ip所对应的port
        conver_port = port_mapping.get(did)
        # 根据did匹配不同关节减速比
        conver_ratio = ratio_mapping.get(did)

        maxSpeed_max = 15000
        maxSpeed_min = 1

        if speed > maxSpeed_max or speed < maxSpeed_min:
            print('-- 旋转电机ID：%s 运行速度错误，输入运行速度：%s，实际范围：%s~%s' % (did, speed, maxSpeed_min, maxSpeed_max))
            return

        # 加速度上下限
        acceleration_max = 200
        acceleration_min = 1
        if acceleration > acceleration_max or acceleration < acceleration_min:
            print('-- 旋转电机ID：%s 加速度错误，输入加速度：%s，实际范围：%s~%s' % (did, acceleration, acceleration_min, acceleration_max))
            return

        # 检查电机限位
        max_min = motor_max_min_limit.get(did)
        if max_min is None:
            print('-- 旋转电机ID错误，输入电机ID：%s' % did)
            return

        # 运行角度上下限
        max_v = max_min[0]
        min_v = max_min[1]
        if angleControl > max_v or angleControl < min_v:
            print('-- 旋转电机ID：%s 角度错误，输入角度：%s，实际范围：%s~%s' % (did, angleControl, max_v, min_v))
            return

        run_command = self.dian_control_command(did, angleControl, speed, acceleration, conver_ratio)
        if isPrint == True:
            print('-- 旋转电机执行角度命令: %s' % run_command)

        self.dian_send_command(ip, conver_port, run_command)


    # 电机停机，回待机状态命令
    # did 电机ID 1~10
    def SetMotorStop(self, did, isPrint=False):
        ip = ip_mapping.get(did)
        run_command = self.dian_stop_command()
        if isPrint == True:
            print('-- 电机停止命令: %s' % run_command)
        self.dian_send_command(did, run_command, ip)


    # 获取电机控制板信息（常用）
    def GetMotorData(self, did, isPrint=False):
        rlt = (0, 0.0)
        try:
            conver_port = port_mapping.get(did)
            ip = ip_mapping.get(did)
            run_command = self.dian_data_command()
            if isPrint == True:
                print('-- 获取电机角度命令: %s' % run_command)
            rlt = self.dian_send_command_r(did, ip, conver_port, run_command)
        except:
            pass

        return rlt


    # 获取电机上下限信息（不常用）
    def GetMotorLimit(self, did, isPrint=False):
        rlt = (0, (0, 0))
        try:
            ip = ip_mapping.get(did)
            conver_port = port_mapping.get(did)
            run_command = self.dian_data_command()
            if isPrint == True:
                print('-- 获取电机上下限命令: %s' % run_command)
            rlt = self.dian_send_command_r2(did, ip, conver_port, run_command)
        except:
            pass

        return rlt


    # 设置零点命令（不常用）
    def SetZero(self, did, isPrint=False):
        ip = ip_mapping.get(did)
        conver_port = port_mapping.get(did)
        run_command = self.dian_zero_command()
        if isPrint == True:
            print('-- 设置电机零点命令: %s' % run_command)
        self.dian_send_command(ip, conver_port, run_command)


    # 设置限位命令（不常用）
    def SetLimit(self, did, low, high, isPrint=False):
        ip = ip_mapping.get(did)
        conver_port = port_mapping.get(did)
        run_command = self.dian_limit_command(low, high)
        if isPrint == True:
            print('-- 设置电机上下限命令: %s' % run_command)
        self.dian_send_command(ip, conver_port, run_command)


    # 获取电机电流
    def GetCUR(self, did, isPrint=False):
        rlt = (0, 0)
        try:
            ip = ip_mapping.get(did)
            conver_port = port_mapping.get(did)
            run_command = self.dian_cur_command()
            if isPrint == True:
                print('-- 获取电机电流命令: %s' % run_command)
            rlt = self.dian_send_command_r1(did, ip, conver_port, run_command)
        except:
            pass

        return rlt


# 电机限位，直线电机限制是行程单位是mm，旋转电机限制是度数
motor_max_min_limit = {
    11: (10, -10),
    12: (15, -10),
    13: (335, 271),
    14: (335, 271),
    15: (337, 256),
    16: (337, 256),

    21: (10, -10),
    22: (10, -15),
    23: (335, 271),
    24: (335, 271),
    25: (337, 256),
    26: (337, 256),

    31: (125, -125),
    32: (-5, -90),
    33: (120, -120),
    34: (260, 211),
    35: (120, -120),
    36: (196, 160),
    37: (196, 160),

    41: (125, -125),
    42: (90, 5),
    43: (120, -120),
    44: (260, 211),
    45: (120, -120),
    46: (196, 160),
    47: (196, 160),

    71: (20, -20), #旋转
    72: (10, -10), #摇摆
}


if __name__ == '__main__':
    rotaryMotorNet = RotaryMotorNet()
    # 1、电机运动命令
    # 入参1：did = 电机ID
    # 入参2：angleControl = 运行角度
    # 入参3(直线电机)：maxSpeed = 运行速度，默认值5000，调整范围：1-15000
    # 入参4：acceleration = 加速度，默认值100，调整范围：1-200
    # 入参5：isPrint = 是否打印封装命令，True为打印、False为不打印，默认为False不打印
    # 返回参数：无
    # 旋转电机限位参考（单位°）：
    # 1）11：10~-10
    # 2）12：15~-10
    # 3）21：10~-10
    # 4）22：10~-15
    # 5）71：20~-20
    # 6）72：10~-10
    # 7）31：125~-125
    # 8）32：0~-90
    # 9）33：120~-120
    # 10）35：120~-120
    # 11）41：125~-125
    # 12）42：90~0
    # 13）43：120~-120
    # 14）45：120~-120
    rotaryMotorNet.SetMotorData(12, 0, 5000, 200)

    # 2、获取电机当前角度（常用）
    # 入参1：did = 电机ID
    # 入参2：isPrint = 是否打印封装命令，True为打印、False为不打印，默认为False不打印
    # 返回参数：  （stats, real_angle），当status为1时，参数为可用
    # 不可用返回值 (0,     0)
    # 可用返回值   (1,    10)
    rlt = rotaryMotorNet.GetMotorData(74)
    print(rlt)
    # time.sleep(1)

    # 3、设置电机零点（不常用）
    # 入参1：did = 电机ID
    # 入参2：isPrint = 是否打印封装命令，True为打印、False为不打印，默认为False不打印
    # 返回参数：无
    # rotaryMotorNet.SetZero(26)

    # 4、设置电机限制命令（不常用）
    # 入参1：did = 电机ID
    # 入参2：low = 低位限定值
    # 入参3：high = 高位限定值
    # 入参4：isPrint = 是否打印封装命令，True为打印、False为不打印，默认为False不打印
    # rotaryMotorNet.SetLimit(22, 3000, 15000)

    # 5、获取电机电流命令（不常用）
    # 入参1：did = 电机ID
    # 入参2：isPrint = 是否打印封装命令，True为打印、False为不打印，默认为False不打印
    # rlt = rotaryMotorNet.GetCUR(32)
    # print(rlt)

    # 6、获取电机上下限值（不常用）
    # 入参1：did = 电机ID
    # 入参2：isPrint = 是否打印封装命令，True为打印、False为不打印，默认为False不打印
    # 返回参数：  （stats, （limit_min，limit_max）），当status为1时，参数为可用
    # 不可用返回值 (0,     （0,0）)
    # 可用返回值   (1,    （-1600,1600）)
    # rlt = rotaryMotorNet.GetMotorLimit(25)
    # print(rlt)